clear;close all;clc;
dbstop if error;
%张正友标定复现
%实验环境：
%CPU：I7-13700KF
%硬盘：SamSung 970EVO Plus 1T
%平台：Matlab 2024a  至少2021b
%%
ImagePoints=load("ProjectorPoints.mat").ProjectorPoints;
mkdir("result_pic");
%生成世界坐标
%模式确定
[~,~,numImages]=size(ImagePoints);
detector=vision.calibration.monocular.SymmetricCircleGridDetector();
%标定板有9行11列 以用来确定x 轴和y轴 9为为y方向 11为x方向
PatternDims=[9,11];
centerDistance=1.5;
WorldPoints=generateWorldPoints(detector,'PatternDims',PatternDims,'CenterDistance',centerDistance);
N_point=PatternDims(1)*PatternDims(2);
%world coordinates
M = zeros(3,N_point);
M(:,:)=[WorldPoints,ones(length(WorldPoints),1)]';
%%
%计算单应性矩阵
H = zeros(3,3,numImages);
A=zeros(PatternDims(1)*PatternDims(2)*2,8,numImages);%对于采集到的每一幅标定图案 
B=zeros(PatternDims(1)*PatternDims(2)*2,1,numImages);%对于采集到的每一幅标定图案 
res_Homography=zeros(1,numImages);
% [ u1  ] = [ x1  y1  1  0   0   0  -u1*x1  -u1*y1 ] * [H11]
% [ u2  ] = [ x2  y2  1  0   0   0  -u2*x2  -u2*y2 ]   [H12]
% [ u3  ] = [ x3  y3  1  0   0   0  -u3*x3  -u3*y3 ]   [H12]
% [ u1  ] = [ x4  y4  1  0   0   0  -u4*x4  -u4*y4 ]   [H21]
% [ ... ]   [ ...                                  ]   [H22]
% [ un  ] = [ xn  yn  1  0   0   0  -un*xn  -un*yn ]   [h23]
% [ v1  ] = [ 0   0   0  x1  y1  1  -v1*x1  -v1*y1 ]   [H31]
% [ v2  ] = [ 0   0   0  x2  y2  1  -v2*x2  -v2*y2 ]   [H32]
% [ v3  ] = [ 0   0   0  x3  y3  1  -v3*x3  -v3*y3 ]
% [ v4  ] = [ 0   0   0  x4  y4  1  -v4*x4  -v4*y4 ]
% [ ... ]   [ ...                                  ]  
% [ vn  ] = [ 0   0   0  xn  yn  1  -vn*xn  -vn*yn ]
for i=1:numImages%控制张数
   World_cent = mean(WorldPoints, 1);
   Image_cent = mean(ImagePoints(:,:,i), 1);
   WorldPoints_Norm=bsxfun(@minus,WorldPoints,World_cent);
   ImagePoints_Norm=bsxfun(@minus,ImagePoints(:,:,i),Image_cent);
World_sumOfPointDistancesFromOriginSquared = sum(hypot(WorldPoints_Norm(:,1),WorldPoints_Norm(:,2)).^2 );%https://ww2.mathworks.cn/help/matlab/ref/hypot.html
Image_sumOfPointDistancesFromOriginSquared = sum(hypot(ImagePoints_Norm(:,1),ImagePoints_Norm(:,2)).^2 );
if World_sumOfPointDistancesFromOriginSquared > 0
    World_scaleFactor = sqrt(2*N_point) / sqrt(World_sumOfPointDistancesFromOriginSquared);
else
    % If all input control points are at the same location, the denominator
    % of the scale factor goes to 0. Don't rescale in this case.
    World_scaleFactor = 1.0;
end
if Image_sumOfPointDistancesFromOriginSquared > 0
    Image_scaleFactor = sqrt(2*N_point) / sqrt(Image_sumOfPointDistancesFromOriginSquared);
else
    % If all input control points are at the same location, the denominator
    % of the scale factor goes to 0. Don't rescale in this case.
    Image_scaleFactor = 1.0;
end

   % Scale control points by a common scalar scale factor
   WorldPoints_Norm = WorldPoints_Norm .* World_scaleFactor;

   % Scale control points by a common scalar scale factor
   ImagePoints_Norm = ImagePoints_Norm .* Image_scaleFactor;
   World_normMatrixInv = [...
        1/World_scaleFactor,     0,            0;...
        0,            1/World_scaleFactor,     0;...
        World_cent(1),      World_cent(2),      1];

   Image_normMatrixInv = [...
        1/Image_scaleFactor,     0,            0;...
        0,            1/Image_scaleFactor,     0;...
        Image_cent(1),      Image_cent(2),      1];

x = ImagePoints_Norm(:,1);
y = ImagePoints_Norm(:,2);
vec_1 = ones(N_point,1);
vec_0 = zeros(N_point,1);
u = WorldPoints_Norm(:,1);
v = WorldPoints_Norm(:,2);
B(:,:,i) = [u; v];
A(:,:,i) = [x      y      vec_1  vec_0  vec_0  vec_0  -u.*x  -u.*y;
     vec_0  vec_0  vec_0  x      y      vec_1  -v.*x  -v.*y  ];
% [1] Hartley, R; Zisserman, A. "Multiple View Geometry in Computer Vision." 
% Cambridge University Press, 2003. pg. 180-181.
   H0 = A(:,:,i)\B(:,:,i); 
   % We assumed I = 1;
   H0(9) = 1;
   H0 = reshape(H0,3,3);
   H0 = Image_normMatrixInv \ (H0 * World_normMatrixInv);
   H0 = inv(H0);
   H0 = H0 ./ H0(3,3);
   H0=H0';
% options=optimset('Algorithm','levenberg-marquardt','Display','iter','PlotFcn','optimplotresnorm','ScaleProblem','jacobian' );
% [x,res] = lsqnonlin(@fun_Homography, H0, [], [], options, ImagePoints(:,:,i), M);
% res_Homography(:,i) = res;
% H(:,:,i)=x;
  H(:,:,i)=H0;
end

% Loop over images
h = axes('Parent',figure);
for i=1:numImages%控制张数
    m_ = H(:,:,i)*M;
    m_ = [m_(1,:)./m_(3,:);m_(2,:)./m_(3,:);m_(3,:)./m_(3,:)];
    ReProjectionError = ImagePoints(:,:,i)' - m_(1:2,:);
    ReProjectionError=ReProjectionError';
    % scatter(ImagePoints(:,1,i),ImagePoints(:,2,i),'+','ColorVariable',[rand(),rand(),rand()]);
    % hold on;
    scatter(ReProjectionError(:,1),ReProjectionError(:,2),'+','ColorVariable',[rand(),rand(),rand()]);
    hold on;
end

title('Stage1CameraHomography','FontWeight','bold','FontName','Times New Roman');
ylabel(' Error in Y(Pixel)','FontWeight','bold','FontName','Times New Roman');
% 创建 xlabel
xlabel('Error in X(Pixel)','FontWeight','bold','FontName','Times New Roman');
% 设置其余坐标区属性
set(h,'FontName','Times New Roman','FontSize',15,'FontWeight','bold',...
        'LineWidth',2);
box(h,'on');
set(h,'LineWidth',2) 
grid(h,'on');
hold(h,'off');
exportgraphics(h,'result_pic/Stage1CameraHomography.png','Resolution',600);
temp=[];
clear i ;
clear A ;clear B;
clear World_cent ;clear Image_cent;
clear Image_scaleFactor ;clear World_scaleFactor;
clear Image_normMatrixInv ;clear World_normMatrixInv;
clear x ;clear y;clear vec_1 ;clear vec_0;clear u; clear v; 
clear Image_sumOfPointDistancesFromOriginSquared; clear World_sumOfPointDistancesFromOriginSquared; 
clear H0; 
%%
%计算内参数 经典V B 计算
V = zeros(2*numImages,6);
for i=1:numImages
    V(2*i-1,:)=[H(1,1,i)*H(1,2,i) H(1,1,i)*H(2,2,i)+H(2,1,i)*H(1,2,i) ...
        H(2,1,i)*H(2,2,i) H(3,1,i)*H(1,2,i)+H(1,1,i)*H(3,2,i) ...
        H(3,1,i)*H(2,2,i)+H(2,1,i)*H(3,2,i) H(3,1,i)*H(3,2,i)];
    v1=[H(1,1,i)^2 2*H(2,1,i)*H(1,1,i) H(2,1,i)^2 2*H(3,1,i)*H(1,1,i) 2*H(3,1,i)*H(2,1,i) H(3,1,i)^2];
    v2=[H(1,2,i)^2 2*H(2,2,i)*H(1,2,i) H(2,2,i)^2 2*H(3,2,i)*H(1,2,i) 2*H(3,2,i)*H(2,2,i) H(3,2,i)^2];
    V(2*i,:)=v1-v2;
end
[~, ~, U] = svd(V);
b = U(:, end);
% b = [B11, B12, B22, B13, B23, B33]
B =[];
B = [b(1), b(2), b(4);
    b(2), b(3), b(5); 
    b(4), b(5), b(6)];
cy = (B(1,2)*B(1,3) - B(1,1)*B(2,3)) / (B(1,1)*B(2,2)-B(1,2)^2);
lambda = B(3,3) - (B(1,3)^2 + cy * (B(1,2)*B(1,3) - B(1,1)*B(2,3))) / B(1,1);
fx = sqrt(lambda / B(1,1));
fy = sqrt(lambda * B(1,1) / (B(1,1) * B(2,2) - B(1,2)^2));
skew = -B(1,2) * fx^2 * fy / lambda;
cx = skew * cy / fy - B(1,3) * fx^2 / lambda;
Intrinsics =[fx,skew,cx;...
             0,fy,cy; ...
                0,0,1];
% Initialize principal points with centroid of image points if they
% are negative.
if cx < 0 || cy < 0
     % Compute the centroid of all image points. 
    % imagePoints is a numTargetPoints-by-2-numImages-by-numCameras vector
    % centroid is a 1-by-2 vector
    % Reorder the dimensions of imagePoints to 
    % numTargetPoints-by-numImages-by-numCameras-by-2.
    allImagePoints = permute(ImagePoints, [1 3 4 2]);
    % Reshape the imagePoints to numTargetPoints*numImages*numCameras-by-2
    sz = size(allImagePoints);
    allImagePoints = reshape(allImagePoints, [prod(sz(1:3)), sz(4)]);%prod函数为https://ww2.mathworks.cn/help/matlab/ref/prod.html
    % Remove any NaN points that correspond to partial detections.
    validDetectionsIdx = ~any(isnan(allImagePoints),2);
    validImagePoints = allImagePoints(validDetectionsIdx,:);
    % Compute centroid using valid image points.
    centroid = mean(validImagePoints);
    Intrinsics(1:2,3) =centroid;
end
clear V ;clear v1;clear v2;clear U;clear B;
%%
%计算外参数
Intrinsicsinv = inv(Intrinsics);
rotationVectors = zeros(3, numImages);
translationVectors = zeros(3, numImages); 
for i = 1:numImages
    temp = H(:, :, i);
    h1 = temp(:, 1);
    h2 = temp(:, 2);
    h3 = temp(:, 3);
    lambda = 1 / norm(Intrinsicsinv * h1); 
    % 3D rotation matrix
    r1 = lambda * Intrinsicsinv * h1; 
    r2 = lambda * Intrinsicsinv * h2; 
    r3 = cross(r1, r2);
    R = [r1,r2,r3];  
    rotationVectors(:, i) = rotationMatrixToVector(R);
    % translation vector
    t = lambda * Intrinsicsinv * h3;
    translationVectors(:, i) = t;
end
rotationVectors = rotationVectors';
translationVectors = translationVectors';
% Loop over images
h = axes('Parent',figure);
Intrinsics=[fx,0,cx;...
             0,fy,cy; ...
                0,0,1];
for i=1:numImages%控制张数
    % Convert rotation matrix
    R = rotationVectorToMatrix(rotationVectors(i,:)); % MATLAB defines rotation matrix with row vectors
    vT = translationVectors(i,:);
    RT_ = [R(:,1) R(:,2) vT'];
    m_ = Intrinsics*RT_*M;
    m_ = [m_(1,:)./m_(3,:);m_(2,:)./m_(3,:);m_(3,:)./m_(3,:)];
    m_=m_';
    ReProjectionError(:,:,i) = ImagePoints(:,:,i) - m_(:,1:2);
    scatter(ReProjectionError(:,1,i),ReProjectionError(:,2,i),'+','ColorVariable',[rand(),rand(),rand()]);
    hold on;
end

title('Stage2CameraIntrinsicsRT','FontWeight','bold','FontName','Times New Roman');
ylabel(' Error in Y(Pixel)','FontWeight','bold','FontName','Times New Roman');
% 创建 xlabel
xlabel('Error in X(Pixel)','FontWeight','bold','FontName','Times New Roman');
% 设置其余坐标区属性
set(h,'FontName','Times New Roman','FontSize',15,'FontWeight','bold',...
        'LineWidth',2);
box(h,'on');
set(h,'LineWidth',2) 
grid(h,'on');
hold(h,'off');
exportgraphics(h,'result_pic/Stage2CameraIntrinsicsRT.png','Resolution',600);

temp=[];
clear i ;
clear A ;clear B;
clear World_cent ;clear Image_cent;
clear Image_scaleFactor ;clear World_scaleFactor;
clear Image_normMatrixInv ;clear World_normMatrixInv;
clear x ;clear y;clear vec_1 ;clear vec_0;clear u; clear v; 
clear Image_sumOfPointDistancesFromOriginSquared; clear World_sumOfPointDistancesFromOriginSquared; 
clear H0; 
clear temp;

% 线性优化层
params =[];
params =[params,fx,cx,fy,cy];%skew默认为0 你想用自己加进去
for i = 1:numImages
    RT=[rotationVectors(i,:),translationVectors(i,:)];
    params=[params,RT];
end
options=optimset('Algorithm','levenberg-marquardt','Display','iter','PlotFcn','optimplotresnorm','ScaleProblem','jacobian' );
[x,res] = lsqnonlin(@fun_Linear_parameter_optimization, params, [], [], options, ImagePoints, M);


fx=x(1);
cx=x(2);
fy=x(3);
cy=x(4);
Intrinsics=[fx,0,cx;...
             0,fy,cy; ...
                0,0,1];
% Loop over images
h = axes('Parent',figure);
for i = 1:numImages
        RT = x(1,5+(i-1)*6:10+(i-1)*6);
        vRot = RT(1,1:3);
        vT = RT(1,4:6);
        rotationVectors(i,:)=vRot;
        translationVectors(i,:)=vT;

        % Convert rotation matrix
        R = rotationVectorToMatrix(vRot); % MATLAB defines rotation matrix with row vectors
        rotationMatrices(:,:,i)=R;
        translationVectors(i,:)=vT;

        RT_ = [R(:,1) R(:,2) vT'];
        m_ = Intrinsics*RT_*M;
        m_=[m_(1,:)./m_(3,:);m_(2,:)./m_(3,:);m_(3,:)./m_(3,:)]';
        LinearImagePoints(:,:,i)=m_(:,1:2);
        
        ReProjectionError(:,:,i)=ImagePoints(:,:,i)-m_(:,1:2);
        temp=sqrt(ReProjectionError(:,1,i).^2+ReProjectionError(:,2,i).^2);
        MeanReProjectionError(i,1)=mean(temp);
        scatter(ReProjectionError(:,1,i),ReProjectionError(:,2,i),'+','ColorVariable',[rand(),rand(),rand()]);
        hold on; 
end



mean_val=mean(MeanReProjectionError);
rectangle('Position',[0-mean_val,0-mean_val,2*mean_val,2*mean_val],'Curvature',[1 1],'EdgeColor','red','LineWidth',2);
scatter(0,0,'r*','LineWidth',2);

title('Stage3LinearOptimalCamera','FontWeight','bold','FontName','Times New Roman');
ylabel(' Error in Y(Pixel)','FontWeight','bold','FontName','Times New Roman');
% 创建 xlabel
xlabel('Error in X(Pixel)','FontWeight','bold','FontName','Times New Roman');
% 设置其余坐标区属性
set(h,'FontName','Times New Roman','FontSize',15,'FontWeight','bold',...
        'LineWidth',2);
box(h,'on');
set(h,'LineWidth',2) 
grid(h,'on');
hold(h,'off');
exportgraphics(h,'result_pic/Stage3LinearOptimalCamera.png','Resolution',600);
temp=[];


%%
%初始化畸变参数
params =[];
params =[params,fx,cx,fy,cy];%skew默认为0 你想用自己加进去
radialCoeffs=[0,0];
TangentialCoeffs=[0,0];
params =[params,radialCoeffs,TangentialCoeffs];
for i = 1:numImages
    RT=[rotationVectors(i,:),translationVectors(i,:)];
    params=[params,RT];
end
options=optimset('Algorithm','levenberg-marquardt','Display','iter','PlotFcn','optimplotresnorm','ScaleProblem','jacobian' );
[x,res] = lsqnonlin(@fun_Global_parameter_optimization, params, [], [], options, ImagePoints, M);
exportgraphics(gcf,'result_pic/levenberg-marquardt-result.png','Resolution',600);


Intrinsics = [x(1) 0 x(2);...
        0 x(3) x(4); ...
        0 0 1];
radialCoeffs=[x(5),x(6)];
TangentialCoeffs=[x(7),x(8)];



temp=[];
UV=[];
% Loop over images
h = axes('Parent',figure);
 for i = 1:numImages
        RT = x(1,9+(i-1)*6:14+(i-1)*6);
        vRot = RT(1,1:3);
        vT = RT(1,4:6);
        % Convert rotation matrix
        R = rotationVectorToMatrix(vRot); % MATLAB defines rotation matrix with row vectors
        rotationMatrices(:,:,i)=R;
        translationVectors(i,:)=vT;

        RT_ = [R(:,1) R(:,2) vT'];
        m_ = Intrinsics*RT_*M;
        m_=[m_(1,:)./m_(3,:);m_(2,:)./m_(3,:);m_(3,:)./m_(3,:)]';
        UV2(:,:,i)=distortPoints(m_(:,1:2),Intrinsics,radialCoeffs,TangentialCoeffs);
        ReProjectionError(:,:,i)=ImagePoints(:,:,i)-UV2(:,:,i);
        UV=[UV;ReProjectionError(:,:,i)];
        temp=sqrt(ReProjectionError(:,1,i).^2+ReProjectionError(:,2,i).^2);
        MeanReProjectionError(i,1)=mean(temp);
        scatter(ReProjectionError(:,1,i),ReProjectionError(:,2,i),'+','ColorVariable',[rand(),rand(),rand()]);
        hold on; 
 end




%%
mean_val=mean(MeanReProjectionError);
rectangle('Position',[0-mean_val,0-mean_val,2*mean_val,2*mean_val],'Curvature',[1 1],'EdgeColor','red','LineWidth',2);
scatter(0,0,'r*','LineWidth',2);

    title('Stage4Camera','FontWeight','bold','FontName','Times New Roman');
    ylabel(' Error in Y(Pixel)','FontWeight','bold','FontName','Times New Roman');
    % 创建 xlabel
    xlabel('Error in X(Pixel)','FontWeight','bold','FontName','Times New Roman');
    % 设置其余坐标区属性
    set(h,'FontName','Times New Roman','FontSize',15,'FontWeight','bold',...
        'LineWidth',2);
    box(h,'on');
    set(h,'LineWidth',2) 
    grid(h,'on');
    hold(h,'off');
    exportgraphics(h,'result_pic/Stage4Camera.png','Resolution',600);
    temp=[];



order=1:numImages;
h = axes('Parent',figure);
bar(order,MeanReProjectionError'),title("Mean Reprojection Error per Image");
hold on;
plot([0,numImages+1],[mean_val,mean_val],'--');
text(numImages,mean_val,'Mean Error');
legend("Overall Mean Error:  "+num2str(mean_val)+" pixels",'Location','southeast');
    ylabel('Mean Error in Pixels','FontWeight','bold','FontName','Times New Roman');
    % 创建 xlabel
    xlabel('Images','FontWeight','bold','FontName','Times New Roman');
    % 设置其余坐标区属性
    set(h,'FontName','Times New Roman','FontSize',15,'FontWeight','bold',...
        'LineWidth',2);
    box(h,'on');
    set(h,'LineWidth',2) 
    grid(h,'on');
    hold(h,'off');
exportgraphics(h,'result_pic/Mean Reprojection Error per Image.png','Resolution',600);



% Loop over images
h = axes('Parent',figure);

histogram(UV(:,1),  'DisplayStyle','stairs',...
    'Orientation','horizontal',...
    'LineWidth',2,'FaceColor','none',...
    'BinMethod','auto');
legend("Without Bundle Adjustment",'Location','southeast');
ylabel('Error in u Direction(in pixel)','FontWeight','bold','FontName','Times New Roman');
% 创建 xlabel
xlabel('Number of points','FontWeight','bold','FontName','Times New Roman');
    % 设置其余坐标区属性
    set(h,'FontName','Times New Roman','FontSize',15,'FontWeight','bold',...
        'LineWidth',2);
    box(h,'on');
    set(h,'LineWidth',2) 
    grid(h,'on');
    hold(h,'off');
exportgraphics(h,'result_pic/Error in u Direction(in pixel).png','Resolution',600);
    % Loop over images
h = axes('Parent',figure);

histogram(UV(:,2),  'DisplayStyle','stairs',...
    'Orientation','horizontal',...
    'LineWidth',2,'FaceColor','none',...
    'BinMethod','auto');
legend("Without Bundle Adjustment",'Location','southeast');
ylabel('Error in v Direction(in pixel)','FontWeight','bold','FontName','Times New Roman');
% 创建 xlabel
xlabel('Number of points','FontWeight','bold','FontName','Times New Roman');
    % 设置其余坐标区属性
    set(h,'FontName','Times New Roman','FontSize',15,'FontWeight','bold',...
        'LineWidth',2);
    box(h,'on');
    set(h,'LineWidth',2) 
    grid(h,'on');
    hold(h,'off');

exportgraphics(h,'result_pic/Error in v Direction(in pixel).png','Resolution',600);